#include <ros/ros.h>
#include <iostream>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <gnc_functions2.hpp>
#include <ros/time.h> 
#include <ros/duration.h>
#include <std_msgs/Bool.h>
#include <cmath>
#include <ar_track_alvar_msgs/AlvarMarkers.h>
#include <ar_track_alvar_msgs/AlvarMarker.h>
#define PI 3.14159265358979323846
using namespace Eigen;
using namespace std;

Eigen::Isometry3d Twa,Tca,Tbc,Twb,Twc;//刚体变换矩阵
Eigen::Matrix3d rotation_matrix_bc,rotation_matrix_wb,rotation_matrix_ca;;
Eigen::Vector3d t_wb,t_ca;
Quaterniond q_wb,q_ca;

ros::Time last_request;//
ros::Time land_request;//

std::vector<std::pair<double, double>> waypoints;
int current_waypoint_index = 0;

bool countonlytime=true;//防止land_request一直更新
int count3=0;
bool count1time=true;//防止count3一次多加
double distancetp=0;
double Azimuth=0;
float deg2rad = (M_PI/180);
float ground_alt=0;
const double HOVER_RADIUS = 0.00001;   // 经纬度偏移量（约11米）
const int HOVER_POINTS = 4;           // 盘旋航点数
vector<pair<double, double>> hover_waypoints;  // 盘旋航点容器
bool hover_waypoints_generated = false;  

void generate_hover_waypoints(double center_lat, double center_lon)
{
    // 生成四个方向的盘旋航点
    hover_waypoints.clear();
    hover_waypoints.push_back({center_lat + HOVER_RADIUS, center_lon});  // 北
    hover_waypoints.push_back({center_lat, center_lon + HOVER_RADIUS});  // 东
    hover_waypoints.push_back({center_lat - HOVER_RADIUS, center_lon});  // 南
    hover_waypoints.push_back({center_lat, center_lon - HOVER_RADIUS});  // 西
}


double toRadians(double degree) {
    return degree * PI / 180.0;
}

// 将弧度转换为度数
double toDegrees(double radian) {
    return radian * 180.0 / PI;
}

// 计算方位角（方向角）
double calculateAzimuth(double lat1, double lon1, double lat2, double lon2) {
    // 将经纬度从度数转换为弧度
    lat1 = toRadians(lat1);
    lon1 = toRadians(lon1);
    lat2 = toRadians(lat2);
    lon2 = toRadians(lon2);

    double deltaLon = lon2 - lon1;

    // 使用公式计算方向角
    double x = sin(deltaLon) * cos(lat2);
    double y = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(deltaLon);

    double azimuth = atan2(x, y);  // 结果为弧度

    // 将方位角从弧度转换为度数
    double azimuthDeg = toDegrees(azimuth);

    // 调整结果到 0-360 度范围内
    if (azimuthDeg < 0) {
        azimuthDeg += 360.0;
    }

    return azimuthDeg;
}

void scan_cb(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    ROS_INFO("avoid=%d",avoid);
    ROS_INFO("Azimuth=%f",Azimuth);
    sensor_msgs::LaserScan current_2D_scan;
    current_2D_scan=*msg;
    float avoidance_vector_x=0;
    float avoidance_vector_y=0;

    avoid=false;

    float d0 = 3.5; 
    float k = 0.5;
    for(int i=1; i<current_2D_scan.ranges.size(); i++)
    {
        
        if(current_2D_scan.ranges[i] < d0 && current_2D_scan.ranges[i] >.35)
        {
            avoid = true;
            float x = cos(current_2D_scan.angle_increment*i);
            float y = sin(current_2D_scan.angle_increment*i);
            float U = -.5*k*pow(((1/current_2D_scan.ranges[i]) - (1/d0)), 2);   

            avoidance_vector_x = avoidance_vector_x + x*U;
            avoidance_vector_y = avoidance_vector_y + y*U;
        }
    }
    float ux=cos(Azimuth*deg2rad);
    float uy=sin(Azimuth*deg2rad);
    float Uat=-.5*k*pow(distancetp,2);
    float att_vector_x=Uat*ux;
    float att_vector_y=Uat*uy;

    float current_heading = get_current_heading();
  
    avoidance_vector_x = avoidance_vector_x*cos((current_heading)*deg2rad) - avoidance_vector_y*sin((current_heading)*deg2rad);
    avoidance_vector_y = avoidance_vector_x*sin((current_heading)*deg2rad) + avoidance_vector_y*cos((current_heading)*deg2rad);
    
    att_vector_x = att_vector_x*cos((current_heading)*deg2rad) - att_vector_y*sin((current_heading)*deg2rad);
    att_vector_y = att_vector_x*sin((current_heading)*deg2rad) + att_vector_y*cos((current_heading)*deg2rad);
    if(avoid)
    {
        //if( sqrt(pow(avoidance_vector_x,2) + pow(avoidance_vector_y,2)) > 4)
        //{
        //    avoidance_vector_x = 4 * (avoidance_vector_x/sqrt(pow(avoidance_vector_x,2) + pow(avoidance_vector_y,2)));
        //    avoidance_vector_y = 4 * (avoidance_vector_y/sqrt(pow(avoidance_vector_x,2) + pow(avoidance_vector_y,2)));
        //}
        geometry_msgs::Point current_pos;
        current_pos = get_current_location();

        set_destination(att_vector_x+avoidance_vector_x + current_pos.x,att_vector_y+avoidance_vector_y + current_pos.y, 4, 0);   
    }    
}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    t_wb << msg->pose.position.x ,msg->pose.position.y ,msg->pose.position.z;
    //在Eigen库中,四元数的存储顺序是w、x、y、z
    t_wb[2]=t_wb[2]-ground_alt;
    q_wb = Eigen::Quaterniond(msg->pose.orientation.w ,msg->pose.orientation.x ,msg->pose.orientation.y ,msg->pose.orientation.z);
    rotation_matrix_wb = q_wb.toRotationMatrix();
    Twb.linear() = rotation_matrix_wb;
    Twb.translation() = t_wb;
    ROS_INFO("real_alt=%f",t_wb[2]);
}
void aruco_pos_cb(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr &msg)
{
    
    if(!msg->markers.empty()) {
		ROS_INFO("detected");
        
		detect_state=true;
        const auto& first_marker = msg->markers[0];
		
        t_ca << (first_marker.pose.pose.position.y)+0.5*t_wb[2], 
               (first_marker.pose.pose.position.x), 
               0;
               //first_marker.pose.pose.position.z;
        q_ca = Eigen::Quaterniond(
            first_marker.pose.pose.orientation.w,
            first_marker.pose.pose.orientation.x,
            first_marker.pose.pose.orientation.y,
            first_marker.pose.pose.orientation.z
        );
        ROS_INFO("x_error=,%f,y_error=,%f,z_error=,%f",t_ca[0],t_ca[1],t_ca[2]);
    }
	else{
		detect_state=false;
	}
    rotation_matrix_ca = q_ca.toRotationMatrix();
    Tca.linear() = rotation_matrix_ca;  // 获得旋转矩阵
    Tca.translation() = t_ca;           // 平移矩阵 Tca 为变换矩阵
	//ROS_INFO("x,%f,y,%f,z,%f",t_ca[0],t_ca[1],t_ca[2]);
	
}



int main(int argc, char** argv)
{
	//initialize ros 
	ros::init(argc, argv, "gnc_node");
	ros::NodeHandle gnc_node("~");
	init_publisher_subscriber(gnc_node);

	wait4connect();

	wait4start();

	initialize_local_frame();
	Initialize();
	APMLanding APMLand;
    ros::Subscriber aruco_pose_sub = gnc_node.subscribe<ar_track_alvar_msgs::AlvarMarkers>("/ar_pose_marker", 10, aruco_pos_cb);
	ros::Subscriber position_sub = gnc_node.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, pos_cb);  
	mavros_setpoint_pos_pub_ = gnc_node.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
	ros::Subscriber collision_sub = gnc_node.subscribe<sensor_msgs::LaserScan>("/spur/laser/scan", 1, scan_cb);
    //ROS_INFO("real_alt=%f",t_wb[2]);
	takeoff(5);
	//ROS_INFO("current_lat=%f, current_lon=%f", get_current_lat(), get_current_lon());
	//ROS_INFO("getalt=%f",getalt());
	//ROS_INFO("START");
	float start_alt=getalt()-3;
    ground_alt=t_wb[2];
    ROS_INFO("getalt=%f",getalt());
	sleep(3);
    ROS_INFO("real_alt=%f",t_wb[2]);
	t_ca << 99, 99, 99;
    ros::Rate rate(10);
	while(ros::ok())
	{

		ros::spinOnce();
		rate.sleep();

        ROS_INFO("getalt=%f",getalt());
        //desire_vel_ = APMLand.LandingPidProcess(t_ca,0,desire_pose_,0);
        //ROS_INFO("desire_vel=(%f,%f)",desire_vel_[0],desire_vel_[1]);
        //ROS_INFO("calibrating");

        

        // desire_xyVel_[1] = -desire_vel_[0];//机体坐标和相机坐标xy相反
        // if (desire_xyVel_[1]>10){//limit velocity
        //     desire_xyVel_[1]=10;
        // }else if (desire_xyVel_[1]<-10){
        //     desire_xyVel_[1]=-10;
        // } 
        // desire_xyVel_[0] = -desire_vel_[1];
        // if (desire_xyVel_[0]>10){
        //     desire_xyVel_[0]=10;
        // }else if (desire_xyVel_[1]<-10){
        //     desire_xyVel_[0]=-10;
        // }
        // desire_xyVel_[2] = desire_vel_[2];
        // desire_yawVel_ = desire_vel_[3];
        // APMLand.send_body_velxyz_setpoint(desire_xyVel_,desire_yawVel_);
        // sleep(10);
        // ROS_INFO("velocitytest");
        // desire_xyVel_[0]=2;
        // desire_xyVel_[1]=0;
        // desire_xyVel_[2]=0;
        // desire_yawVel_=0;
     
        // APMLand.send_body_posxyz_setpoint(desire_xyVel_,desire_yawVel_);
        //sleep(10);
        ///land();




		
	}
	return 0;
}
